// Devin Koepl, Kevin Kemper

#include "controller_wrapper_states.h"

/*****************************************************************************/

#define STATE_WAKEUP						0
#define STATE_RESTART 						1
#define STATE_CHECK 						2
#define STATE_INITIALIZE 					3
#define STATE_RUN 							4
#define STATE_ERROR 						5

/*****************************************************************************/

static Shm * shm;

/*****************************************************************************/

// Control wrapper variables.


float				last_motor_angle 	= 0.0;
float				last_leg_angle		= 0.0;

float				leg_angle;


static unsigned char command = CMD_RUN;

ControllerInput 	* c_in;
ControllerOutput 	* c_out;

/*****************************************************************************/

// Initialize SHM.  Return true upon success.

unsigned char initialize_shm( void )
{
	// To Uspace SHM
	if ( !( shm = ( Shm * )rt_shm_alloc( nam2num( SHM_NAME ), sizeof( Shm ), USE_VMALLOC ) ) )
		return false;
	memset( shm, 0, sizeof( Shm ) );

	shm->controller_data[0].command 				= shm->controller_data[1].command 				= CMD_DISABLE;
	shm->controller_data[0].controller_requested 	= shm->controller_data[1].controller_requested 	= NO_CONTROLLER;

	return true;
}

/*****************************************************************************/

void takedown_shm( void )
{
	rt_shm_free( nam2num( SHM_NAME ) );
}

/*****************************************************************************/

void control_wrapper_state_machine( uControllerInput ** uc_in, uControllerOutput ** uc_out )
{
	// Keep a copy of the states in memory.
	static unsigned char last_state = STATE_WAKEUP;
	static unsigned char next_state = STATE_WAKEUP;

	//rt_printk( "Next state: %u\n", next_state );

	switch ( next_state )
	{
		case STATE_WAKEUP:
			next_state = state_wakeup( uc_in, uc_out, last_state );
			last_state = STATE_WAKEUP;
			break;
		case STATE_RESTART:
			next_state = state_restart( uc_in, uc_out, last_state );
			last_state = STATE_RESTART;
			break;
		case STATE_CHECK:
			next_state = state_check( uc_in, uc_out, last_state );
			last_state = STATE_CHECK;
			break;
		case STATE_INITIALIZE:
			next_state = state_initialize( uc_in, uc_out, last_state );
			last_state = STATE_INITIALIZE;
			break;
		case STATE_RUN:
			next_state = state_run( uc_in, uc_out, last_state );
			last_state = STATE_RUN;
			break;
		case STATE_ERROR:
			next_state = state_error( uc_in, uc_out, last_state );
			last_state = STATE_ERROR;
			break;
		default:
			next_state = STATE_ERROR;
	}
	
	if ( shm->controller_data[shm->control_index].command == CMD_RESTART ) {
		next_state =  STATE_WAKEUP;
		shm->w_status = 0;
		}

	// Handle controller data change requests.
	if ( shm->req_switch )
	{
		shm->control_index 	= 1 - shm->control_index;
		shm->req_switch 	= false;
	}
}

/*****************************************************************************/

unsigned char state_wakeup( uControllerInput ** uc_in, uControllerOutput ** uc_out, unsigned char last_state )
{

	// Check to see if Medullas are awake by sending them a bad command (0).
	uc_in[0]->command = 0;

	rt_printk( "Check..." );

	// If a Medulla does not report the bad command, then fail.
	if ( !( uc_out[0]->status & STATUS_BADCMD ) )
	{
		rt_printk( "Medulla did not report the bad command. Status: %2X\n", uc_out[0]->status );
		return STATE_WAKEUP;
	}
	rt_printk( "\n" );

	// If successful, Medullas are ready to receive restarts.
	return STATE_RESTART;
}

/*****************************************************************************/

unsigned char state_restart( uControllerInput ** uc_in, uControllerOutput ** uc_out, unsigned char last_state )
{
	rt_printk( "Resetting.\n" );
	// Send restart commands.
	uc_in[0]->command = CMD_RESTART;



	uc_in[0]->MOTOR_TORQUE	= PWM_OPEN;


	if ( last_state == STATE_RESTART )
		return STATE_CHECK;

	return STATE_RESTART;
}

/*****************************************************************************/

unsigned char state_check( uControllerInput ** uc_in, uControllerOutput ** uc_out, unsigned char last_state )
{

	// Send command disables.
	uc_in[0]->command = CMD_DISABLE;

	// Check Medulla states.  They have to be disabled to move on to the next state.
	if ( uc_out[0]->status != STATUS_DISABLED )
	{
		rt_printk( "Medulla not disabled.\n" );
		return STATE_CHECK;
	}

	// Hold the leg open, and the hip floppy.
	uc_in[0]->MOTOR_TORQUE	= PWM_OPEN;


	// Verify Medullas in their correct locations.
	if ( ( uc_out[0]->id != MEDULLA_FCA_ID ) )
	{
		rt_printk( "Medulla in wrong location.\n" );
		return STATE_CHECK;
	}

	return STATE_INITIALIZE;
}

/*****************************************************************************/

// Initialize the encoder counters.
unsigned char state_initialize( uControllerInput ** uc_in, uControllerOutput ** uc_out, unsigned char last_state )
{
	int i;
	
	shm->io_index = 0;
	
	// Tell the control switch to initialize.
	shm->controller_state.state = 3;

	// Create pointers to controller I/O
	c_in	= &shm->controller_input[shm->io_index];
	c_out 	= &shm->controller_output[shm->io_index];

	// Send command disables.
	uc_in[0]->command = CMD_DISABLE;

	// Hold the leg open.
	uc_in[0]->MOTOR_TORQUE	= PWM_OPEN;

	rt_printk( "Running.\n" );
	shm->w_status = 1;

	return STATE_RUN;
}

/*****************************************************************************/

unsigned char state_run( uControllerInput ** uc_in, uControllerOutput ** uc_out, unsigned char last_state )
{
	int i;

	// Create pointers to controller I/O
	c_in	= &shm->controller_input[shm->io_index];
	c_out 	= &shm->controller_output[shm->io_index];

	if ( last_state == state_initialize )
		command = CMD_RUN;
	
	// Send run command.
	for ( i = 0; i < NUM_OF_MEDULLAS_ON_ROBOT; i++ )
	{
		uc_in[i]->command = command;
	}
	// Toggle the run bit.
	command ^= CMD_RUN_TOGGLE_bm;


	// Generate controller input
//	c_in->leg_angle		= UNDISCRETIZE(
//		uc_out[0]->LEG_SEG_ANGLE,
//		MAX_LEG_SEG_ANGLE, MIN_LEG_SEG_ANGLE, MIN_LEG_SEG_COUNT, MAX_LEG_SEG_COUNT);

//	c_in->motor_angle 	= UNDISCRETIZE(
//		uc_out[0]->TRANS_ANGLE,
//		MAX_TRAN_ANGLE, MIN_TRAN_ANGLE, MIN_TRAN_COUNT, MAX_TRAN_COUNT) + TRAN_OFF_ANGLE;


//	UNDISCRETIZE_LOCATION(	COUNT, COUNT_KNOWN, ANGLE_KNOWN, COUNTS_PER_REV, GEAR_RATIO)

//XXX
//	c_in->leg_cnt		= uc_out[0]->LEG_SEG_ANGLE;
	c_in->leg_cnt		= uc_out[0]->LEG_COURSE_ANGLE;
	c_in->motor_cnt		= uc_out[0]->TRANS_ANGLE;

//XXX
//	c_in->leg_angle		= UNDISCRETIZE_LOCATION( uc_out[0]->LEG_SEG_ANGLE, 17551, PI, MAX_13BIT, 5);
	c_in->leg_angle		= UNDISCRETIZE_LOCATION( uc_out[0]->LEG_COURSE_ANGLE, 8331, PI, MAX_13BIT, 1);

	c_in->motor_angle	= 2*PI - UNDISCRETIZE_LOCATION( uc_out[0]->TRANS_ANGLE, 5401, PI, MAX_13BIT, 5);


			
	c_in->motor_velocity = (c_in->motor_angle - last_motor_angle)
		/ ( (float)uc_out[0]->timestep * SEC_PER_CNT );

	c_in->leg_velocity = (c_in->leg_angle - last_leg_angle)
		/ ( (float)uc_out[0]->timestep * SEC_PER_CNT );


	last_motor_angle	= c_in->motor_angle;
	last_leg_angle		= c_in->leg_angle;
		

	c_in->command = shm->controller_data[shm->control_index].command;

	// Controller update.
	control_switcher_state_machine( c_in, c_out,
		&shm->controller_state, &shm->controller_data[shm->control_index] );
	
	
	// Clamp the motor torques.
	c_out->motor_torque = CLAMP(c_out->motor_torque, MTR_MIN_TRQ, MTR_MAX_TRQ);


	// Send motor torques only when all of the Medullas status's are okay..
	if ( uc_out[0]->status )
	{
		// At least one of the Medullas is unhappy, so don't send motor torques.

		uc_in[0]->MOTOR_TORQUE = DISCRETIZE(
			0.0, MTR_MIN_TRQ, MTR_MAX_TRQ, MTR_MIN_CNT, MTR_MAX_CNT);

		// Print the Medullas' status bytes, because at least one of them has a problem.
		rt_printk( "Status: %u\n", uc_out[0]->status);

		// Move to error state.
		//return state_error;
	}
	else
	{
		// All of the Medullas are okay, so we go ahead and send the torques.

		// Send motor torques.
		uc_in[0]->MOTOR_TORQUE = DISCRETIZE(
			c_out->motor_torque, MTR_MIN_TRQ, MTR_MAX_TRQ, MTR_MIN_CNT, MTR_MAX_CNT);

	}

	// Increment and rollover i/o index for datalogging.
	shm->io_index = (++shm->io_index) % SHM_TO_USPACE_ENTRIES;

	return STATE_RUN;
}

/*****************************************************************************/

unsigned char state_error( uControllerInput ** uc_in, uControllerOutput ** uc_out, unsigned char last_state )
{
	// Send zero motor torques.
	uc_in[0]->MOTOR_TORQUE = DISCRETIZE(
		0.0, MTR_MIN_TRQ, MTR_MAX_TRQ, MTR_MIN_CNT, MTR_MAX_CNT);


	// No coming back from this one yet.
	return STATE_ERROR;
}
